# include <memory>
# include "rclcpp/rclcpp.hpp"
# include "geometry_msgs/msg/transform_stamped.hpp"
# include "tf2_ros/transform_listener.h"
# include "tf2/LinearMath/Quaternion.h"
# include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
# include <chrono>
# include "tf2_ros/buffer.h"
# include "tf2/utils.h"

class TFListener : public rclcpp::Node
{
    private:
    std::shared_ptr<tf2_ros::TransformListener> listener_;
    rclcpp::TimerBase::SharedPtr timer_;
    std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
public:
    TFListener(): Node("tf_listener")
    {
        this->tf_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
        this->listener_ = std::make_unique<tf2_ros::TransformListener>(*tf_buffer_,this);
        timer_ = this->create_wall_timer(std::chrono::milliseconds(10000), std::bind(&TFListener::get_transform, this));
    }
    void get_transform(){
        try{
            const auto transform = tf_buffer_->lookupTransform("map","base_link",this->get_clock()->now(),rclcpp::Duration::from_seconds(1.0f));
            
            auto tranbslation = transform.transform.translation; 
            auto rotation = transform.transform.rotation;
            double y,p,r;
            tf2::getEulerYPR(rotation,y,p,r);
            RCLCPP_INFO(this->get_logger(),"Translation: x: %f, y: %f, z: %f",tranbslation.x,tranbslation.y,tranbslation.z);
            RCLCPP_INFO(this->get_logger(),"Rotation: y: %f, p: %f, r: %f",y,p,r);

        }catch(const std::exception& ex){
            RCLCPP_ERROR(this->get_logger(),"Could not get transform: %s",ex.what());
        }
    }
    // void broadcast_timer_callback(){
    //     geometry_msgs::msg::TransformStamped transform;
    //     transform.header.stamp = this->get_clock()->now();
    //     transform.header.frame_id = "map";
    //     transform.child_frame_id = "base_link";
    //     transform.transform.translation.x = 2.0;
    //     transform.transform.translation.y = 3.0;
    //     transform.transform.translation.z = 0.0;
    //     tf2::Quaternion q;
    //     q.setRPY(0, 0, 30*M_PI/180);
    //     transform.transform.rotation = tf2::toMsg(q);
    //     tf_broadcaster_->sendTransform(transform);
    //     RCLCPP_INFO(this->get_logger(), "Publish TF");
    // }
};

int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<TFListener>());
    rclcpp::shutdown();
    return 0;
}